/*
 * isr.c
 *
 *  Created on: 2017年7月22日
 *      Author: changruowang
 */
#include "system.h"
#include "user_bsp.h"

volatile uint16_t sys_5ms =0;
uint8_t  recOkFlash = 0;
//volatile uint8_t  Task_Timer[3] ={0,0,0};
extern volatile uint16_t En_Wait_time;

void CMT0_Interrupt_Deal()
{

	static uint16_t timeCounter =0;

	sys_5ms ++;

	if(En_Wait_time > 0)
	    En_Wait_time --;
/*
//	Task_Timer[0]++;
	Task_Timer[1]++;
	Task_Timer[2]++;
*/
    if(recOkFlash == 1 && sys_5ms % 10 == 0  )
    {
    	timeCounter++;
    	if(timeCounter >= 10)
    	{
    		timeCounter = 0;
    		recOkFlash = 0;
    		LED_G_H;
    	}
    	else
    		LED_G = ~LED_G;
    }
	if(sys_5ms % 100 == 0)    //500ms TASK
	{
		LED_B = ~LED_B;

	}
	else if(sys_5ms % 6 == 0)   // 25 ms
	{
		SysCtrl_Update_PID();
		SysCtrl_Update_Msg_Ctrl_To_FMU();
	}
/*
	else if(sys_5ms % 7 == 0)   // 30 ms
	{

	}
*/

}

Msg_FMUToCtrl_Pkg_t suiyi;
void MsgFmuToCtrlMy(u8 data)
{

	static u8 state=0;
	static u8 RxBuffer[sizeof(Msg_FMUToCtrl_Pkg_t)];
	static u8 count=0;
	Msg_FMUToCtrl_Pkg_t* p_st=0;
	if(state==0&&data== 0xaa)//
	{
		state=1;
		RxBuffer[0]=data;
	}
	else if(state==1 && data == 0xaa)//
	{
		state=2;
		RxBuffer[1]=data;
		count=0;
	}
	else if(state==2 )
	{
		RxBuffer[2 + count++]=data;
		if(count==sizeof(Msg_FMUToCtrl_Pkg_t)-2)
		{
			state=0;
			p_st=(Msg_FMUToCtrl_Pkg_t*)RxBuffer;
            if(p_st->St==0xaaaa &&p_st->Sp==0x5555)
            {

          			if(p_st->PID_Updata==1)
            		{
          				recOkFlash = 1;
            		    SysPID[Rol].p=-p_st->PID_ROLL_P;
            			SysPID[Rol].i=-p_st->PID_ROLL_I;
            			SysPID[Rol].d=-p_st->PID_ROLL_D;
            			SysPID[Rol].dd=-p_st->PID_ROLL_DD;

            			SysPID[Pit].p=-p_st->PID_PIT_P;
            			SysPID[Pit].i=-p_st->PID_PIT_I;
            			SysPID[Pit].d=-p_st->PID_PIT_D;
            			SysPID[Pit].dd=-p_st->PID_PIT_DD;

            			SysPID[Yaw].p=-p_st->PID_YAW_P;
            			SysPID[Yaw].i=-p_st->PID_YAW_I;
            			SysPID[Yaw].d=-p_st->PID_YAW_D;

            		}
            		Fly_Status_t.ROLL=p_st->Roll;
            		Fly_Status_t.PIT=p_st->PIT;

            		Fly_Status_t.YAW=p_st->YAW;
            		Fly_Status_t.Height=p_st->Alt_Sonar;
            }
		}
	}
	else
	{

		state = 0;
		count =0 ;
	}
}


/*if(Receive_OK==1)
{
		Receive_OK=0;


	if(p_st->St==0xAAAA&&p_st->Sp==0x5555)
	{
	//			if(p_st->PID_Updata==1)
	//	{
				SysPID[Rol].PID[Pos].p=-p_st->PID_ROLL_P;
			SysPID[Rol].PID[Pos].i=-p_st->PID_ROLL_I;
			SysPID[Rol].PID[Pos].d=-p_st->PID_ROLL_D;
			SysPID[Rol].PID[Pos].dd=-p_st->PID_ROLL_DD;

			SysPID[Pit].PID[Pos].p=-p_st->PID_PIT_P;
			SysPID[Pit].PID[Pos].i=-p_st->PID_PIT_I;
			SysPID[Pit].PID[Pos].d=-p_st->PID_PIT_D;
			SysPID[Pit].PID[Pos].dd=-p_st->PID_PIT_DD;

			SysPID[Yaw].PID[Pos].p=-p_st->PID_YAW_P;
			SysPID[Yaw].PID[Pos].i=-p_st->PID_YAW_I;
			SysPID[Yaw].PID[Pos].d=-p_st->PID_YAW_D;

	//	}
		Fly_Status_t.ROLL=p_st->Roll;
		Fly_Status_t.PIT=p_st->PIT;
		Fly_Status_t.YAW=p_st->YAW;
		Fly_Status_t.Height=p_st->Alt_Sonar;

	}
}*/
















